﻿#ifndef MEMATH_H
#define MEMATH_H

#include <math.h>
#include <vector>
#include <algorithm>
#include <float.h>

#define ME_TINY (1e-6)
#define ME_PI (3.14159265358979323846)
#define ME_DEGREE2RAD (3.14159265358979323846/180.0)
#define ME_RAD2DEGREE (180.0/3.14159265358979323846)

#define ME_DOUBLE_MIN -1.0e+299
#define ME_DOUBLE_MAX 1.0e+299

#define ME_IS_ZERO(var) ((var) < ME_TINY && (var) > -ME_TINY)
#define ME_NOT_ZERO(var) ( !((var) < ME_TINY && (var) > -ME_TINY ) )

/** This function converts the radians in double to degrees. */
inline double meRadiansToDegrees(double radians)
{
    return radians * (180.0 / ME_PI);
}
/// Please use MeMath::radiansToDegrees instead.
inline void meRadiansToDegrees(double *radians, double *radiansOut, int size)
{
    for (int i=0; i < size ; i++) {
        radiansOut[i] = radians[i] * (180.0 / ME_PI);
    }
}

/** This function converts the degrees in double to radians. */
inline double meDegreesToRadians(double degrees)
{
    return degrees * (ME_PI / 180.0);
}
inline void meDegreesToRadians(double *degrees, double *degreesOut, int size)
{
    for (int i=0; i < size ; i++) {
        degreesOut[i] = degrees[i] * (ME_PI / 180.0);
    }
}

inline void meTinyToZero(double &tiny)
{
    if (ME_IS_ZERO(tiny)) { tiny = 0; }
}

inline bool meIsEqual(const double &x, const double &y)
{
    return ME_IS_ZERO(x-y);
}

inline double meAcos(const double &x)
{
    double _x = ME_IS_ZERO(x-1.0) ? 1.0: ( ME_IS_ZERO(x+1.0) ? -1.0:x );
    return acos(_x);
}

inline double meAsin(const double &x)
{
    double _x = ME_IS_ZERO(x-1.0) ? 1.0: ( ME_IS_ZERO(x+1.0) ? -1.0:x );
    return asin(_x);
}


class MeMath
{
public:
    /// 弧度到角度转化
    static void radiansToDegrees(const double *radians, double *degrees, int size)
    {
        for (int i=0; i < size ; i++) {
            degrees[i] = radians[i] * (180.0 / ME_PI);
        }
    }

    /// 角度到弧度转化
    static void degreesToRadians(const double *degrees, double *radians, int size)
    {
        for (int i=0; i < size ; i++) {
            radians[i] = degrees[i] * (ME_PI / 180.0);
        }
    }

    static bool isZero(const double &a) {
        return ME_IS_ZERO(a);
    }
    static bool isZeroVec3d(const double a[3]) {
        for (int i = 0; i < 3; ++i)
        {
            if (ME_NOT_ZERO(a[i])) return false;
        }
        return true;
    }
    /**
     * Addition of two 3-vectors (double version). Result is stored in c according to c = a + b.
     */
    static void add(const double a[3], const double b[3], double c[3]) {
        for (int i = 0; i < 3; ++i)
        {
            c[i] = a[i] + b[i];
        }
    }

    /**
     * Subtraction of two 3-vectors (double version). Result is stored in c according to c = a - b.
     */
    static void subtract(const double a[3], const double b[3], double c[3]) {
        for (int i = 0; i < 3; ++i)
        {
            c[i] = a[i] - b[i];
        }
    }

    /// 计算两点间的距离
    static double distance(const double a[3], const double b[3]) {
        return sqrt((b[0]-a[0])*(b[0]-a[0]) + (b[1]-a[1])*(b[1]-a[1]) + (b[2]-a[2])*(b[2]-a[2]));
    }

    /**
     * Multiplies a 3-vector by a scalar (double version).
     * This modifies the input 3-vector.
     */
    static void multiplyScalar(double a[3], double s) {
        for (int i = 0; i < 3; ++i)
        {
            a[i] *= s;
        }
    }

    /**
     * Dot product of two 3-vectors (double version).
     */
    static double dot(const double a[3], const double b[3]) {
        return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
    }

    /**
     * Cross product of two 3-vectors. Result (a x b) is stored in c.
     * (double version)
     */
    static void cross(const double a[3], const double b[3], double c[3]);

    static double pow2(double x0, double y0, double z0)
    {
        return x0*x0 + y0*y0 + z0*z0;
    }
    static double pow2(double x) {
        return x*x;
    }

    /**
      * Compute the norm of 3-vector (double version).
      */
    static double norm(const double v[3]) {
        return sqrt( v[0] * v[0] + v[1] * v[1] + v[2] * v[2] );
    }

    /**
     * Normalize (in place) a 3-vector. Returns norm of vector
     * (double version).
     */
    static double normalize(double v[3]);

    /**
     * Compute the triangle normal from three points.
     * normal direction is （v32XV12).
     */
    static void computeNormal(double v1[3], double v2[3], double v3[3], double n[3]);

    /**
     * Compute the (unnormalized) triangle normal direction from three points.
     * normal direction is （v32XV12).
     */
    static void computeNormalDirection(double v1[3], double v2[3], double v3[3], double n[3]);

    /**
     * Compute angle in radians between two vectors. The unit is radian
     * axis 为输出旋转轴
     */
    static double angleBetweenVectors(const double v1[3], const double v2[3], double axis[3]=nullptr);

    /**
     * Compute the distance of a point x to a finite line (p1,p2).
     */
    static double distanceToLineSeg(const double x[3], const double p1[3], const double p2[3],
            double closestPoint[3]=nullptr);
    /**
     * Compute the distance of a point x to a finite line (p1,p2).
     */
    static double distanceToLineSeg(const double x[3], const double p1[3], const double p2[3], double& t,
            double closestPoint[3]=nullptr);
    /**
     * Determine the distance of the current vertex to the edge defined by
     * the vertices provided.  Returns distance. Note: line is assumed
     * infinite in extent.
     */
    static double distanceToLine(const double x[3], const double p1[3], const double p2[3], double closestPoint[3]=nullptr);

    /**
     * 计算两条线段之间的最短距离
     * 线段 (l0,l1) and (m0,m1).
     * 最短距离点 closestPt1 and closestPt2. 参数t (0 <= t0, t1 <= 1)
     * 返回两条线最短距离
     */
    static double distanceBetweenLineSegments(double l0[3], double l1[3], double m0[3], double m1[3],
      double closestPt1[3], double closestPt2[3], double& t1, double& t2);

    /**
    已知四个点坐标判断是否共面
    可以用行列式来判断
    用四个点求出三个向量分别为(x1,y1,z1),(x2,y2,z2),(x3,y3,z3)
    判断行列式
    | x1 x2 x3|
    | y1 y2 y3|
    | z1 z2 z3|
    或者它的转置是否为零
    若为零则四点共面
    */ 
    static bool isInSamePlane(double p1[3], double p2[3], double p3[3], double p4[3]);

    /// 计算 p 点到平面(p0 - p1 - p2)的距离
    /// \bDirection 是否带方向 (+ /-)
    static double distancePointToPlane(double p[3], double p0[3], double p1[3], double p2[3], const bool bDirection=false);

    /// 计算最小最大值
    template<typename _Tp>
    static int minMax(const std::vector<_Tp> &data, _Tp &minV, _Tp &maxV);

    /// 计算均值, 方差, 均方差
    template<typename _Tp>
    static int meanStdDev(const std::vector<_Tp>& mat, double &mean,
                   double& stddev);

    /// 计算误差的均值 sum(ei)/n, 均方根误差, 最大,最小值
    template<typename _Tp>
    static int meanRmsErr(const std::vector<_Tp>& values, _Tp realV, double &meanErr,
                   double& rmse, double *minErr, double *maxErr);

    /// 计算每一组的均值, 方差, 均方差
    template<typename _Tp>
    static int meanStdDevGroup(const std::vector<std::vector<_Tp>>& mat, std::vector<double> &mean,
                               std::vector<double>& stddev);
};

//----------------------------------------------------------------------------
// Cross product of two 3-vectors. Result (a x b) is stored in c[3].
inline void MeMath::cross(const double a[3], const double b[3], double c[3])
{
    double Cx = a[1] * b[2] - a[2] * b[1];
    double Cy = a[2] * b[0] - a[0] * b[2];
    double Cz = a[0] * b[1] - a[1] * b[0];
    c[0] = Cx; c[1] = Cy; c[2] = Cz;
}

//----------------------------------------------------------------------------
inline double MeMath::normalize(double v[3])
{
    double den = MeMath::norm( v );
    if ( den != 0.0 )
    {
        for (int i=0; i < 3; ++i)
        {
            v[i] /= den;
        }
    }
    return den;
}

//----------------------------------------------------------------------------
inline void MeMath::computeNormalDirection(double v1[3], double v2[3],
                                       double v3[3], double n[3])
{
  double ax, ay, az, bx, by, bz;

  // order is important!!! maintain consistency with triangle vertex order
  ax = v3[0] - v2[0]; ay = v3[1] - v2[1]; az = v3[2] - v2[2];
  bx = v1[0] - v2[0]; by = v1[1] - v2[1]; bz = v1[2] - v2[2];

  n[0] = (ay * bz - az * by);
  n[1] = (az * bx - ax * bz);
  n[2] = (ax * by - ay * bx);
}

//----------------------------------------------------------------------------
inline void MeMath::computeNormal(double v1[3], double v2[3],
                                       double v3[3], double n[3])
{
  double length;

  MeMath::computeNormalDirection(v1, v2, v3, n);

  if ( (length = sqrt((n[0]*n[0] + n[1]*n[1] + n[2]*n[2]))) != 0.0 )
  {
    n[0] /= length;
    n[1] /= length;
    n[2] /= length;
  }
}

inline bool MeMath::isInSamePlane(double p1[3], double p2[3], double p3[3], double p4[3])
{
    double v1[3], v2[3], v3[3];

    subtract(p2, p1, v1);
    subtract(p3, p1, v2);
    subtract(p4, p1, v3);

    double value = (v1[0]*v2[1]*v3[2])+(v1[1]*v2[2]*v3[0])+(v1[2]*v2[0]*v3[1])-(v1[2]*v2[1]*v3[0])-(v2[2]*v3[1]*v1[0])-(v3[2]*v1[1]*v2[0]);
    return isZero(value);
}


inline double MeMath::distancePointToPlane(double p[3], double p0[3], double p1[3], double p2[3], const bool bDirection)
{
    // 通过 p0-p1, p0-p2 计算法线向量，并归一化，然后通过 p0-p在发现上投影计算
    double v1[3], v2[3];
    double norm[3];

    subtract(p1, p0, v1);
    subtract(p2, p0, v2);
    MeMath::cross(v1, v2, norm);

    MeMath::normalize(norm);

    if (bDirection) {
        return (norm[0] * (p[0] - p0[0])  + norm[1] * (p[1] - p0[1]) + norm[2] * (p[2] - p0[2]));
    } else {
        return fabs(norm[0] * (p[0] - p0[0])  + norm[1] * (p[1] - p0[1]) + norm[2] * (p[2] - p0[2]));
    }
}

template<typename _Tp>
inline int MeMath::minMax(const std::vector<_Tp> &data, _Tp &minV, _Tp &maxV)
{
    if (data.size() == 0) {
        return -1;
    }
    minV = data[0];
    maxV = data[0];
    for (unsigned int i=0; i< data.size(); i++) {
        if (data[i] < minV) { minV = data[i]; }
        else if (data[i] > maxV) { maxV = data[i]; }
    }

    return 0;
}

template<typename _Tp>
inline int MeMath::meanStdDev(const std::vector<_Tp>& mat, double &mean,
                double& stddev)
{
    int h = mat.size();

    double scale = 1. / (h);

    double sum = { 0. }, sqsum = { 0. };
    for (unsigned int y = 0; y < h; ++y) {
        double v = static_cast<double>(mat[y]);
        sum += v;
        sqsum += v * v;
    }
    mean = sum * scale;

    double variance = std::max(sqsum*scale -  mean* mean, 0.);
    stddev = std::sqrt(variance);
    return 0;
}

template<typename _Tp>
inline int MeMath::meanRmsErr(const std::vector<_Tp> &values, _Tp realV, double &meanErr, double &rmse, double *minErr, double *maxErr)
{
    uint n = values.size();
    if (n == 0) {
        return -1;
    }

    double scale = 1./ (n);
    double sum = { 0. }, sqsum = { 0. };
    double minv = DBL_MAX, maxv = 0;
    for (unsigned int y = 0; y < n; ++y) {
        double v = static_cast<double>(values[y]-realV);
        sum += v;
        sqsum += v * v;
        if (minv > abs(v)) { minv = abs(v); }
        if (maxv < abs(v)) { maxv = abs(v); }
    }
    meanErr = sum * scale;

    rmse = std::sqrt(sqsum*scale);

    if (minErr) *minErr = minv;
    if (maxErr) *maxErr = maxv;

    return 0;
}

template<typename _Tp>
inline int MeMath::meanStdDevGroup(const std::vector<std::vector<_Tp> > &mat, std::vector<double> &mean, std::vector<double> &stddev)
{
    int h = mat.size(), w = mat[0].size();
    mean.resize(w);
    stddev.resize(w);


    double scale = 1. / (h);
    for (unsigned int x = 0; x < w; ++x) {
        double sum = { 0. }, sqsum = { 0. };
        for (unsigned int y = 0; y < h; ++y) {
            double v = static_cast<double>(mat[y][x]);
            sum += v;
            sqsum += v * v;
        }
        mean[x] = sum * scale;

        double variance = std::max(sqsum*scale - ( mean[x])*( mean[x]), 0.);
        stddev[x] = std::sqrt(variance);
    }

    return 0;
}

#endif // MEMATH_H
